/*************************************************************************
*
*	gps.c
*
*
*  Copyright 2008 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module reads gps data from a gpsd server.
*
*
*  version 0.1, 10/21/08, SBS
*   - first skeletal version 
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/


/*************
* Portions from cgpxlogger.c 4685 2008-02-17 02:57:48Z ckuethe $ 
* those portions under copyright as follows:
* Copyright (c) 2005,2006 Chris Kuethe <chris.kuethe@gmail.com>
*
* Permission to use, copy, modify, and distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*
**************/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <gps.h>

#include "logfile.h"
#include "delay.h"

#include "waypoints.h"
#define NEED_MODENAMES
#include "platform_modes.h"
#undef NEED_MODENAMES
#include "platform_types.h"
#include "status.h"

#include "config.h"
#include "telemfile.h"
#include "sendretr.h"

typedef struct gps_data_t *GPSDATA;

static GPSDATA gpsdata0;
static CONFIGDATA cf0;

#define FILESPECLEN 256
#define CFGFILENAME "gps.cfg"

//void process(struct gps_data_t *, char *, size_t, int);

/*************************
*
*
*************************/
void gpsInterruptCallback(int signum)
{
	#ifdef DEBUG
	char str[LOGSTRLEN];

	snprintf(str,LOGSTRLEN, "\nExiting on signal %d!", signum);
	logfile_LogPrint(str);
	logfile_CloseFile();
	#endif

	printf("\nExiting on signal %d!", signum);
	gps_close(gpsdata0);
	exit(signum);
}


/*************************
*
*
*************************/
static void writeTelemFile(
		CONFIGDATA cf
		,GPSDATA g)
{


	static char filespec[FILESPECLEN];
	static STATUS st;
	static struct tm *tmp;
	static int memory=0;
	
	if(memory==0){
		memory=1;
		
		st = status_InitStatus();
		st.nId = 1;
		snprintf(st.sName,PLATFORMNAMESTRLEN,"GPS");
		st.nMode = MODE_AWAITINSTR;
		snprintf(st.sModeName,sizeof(st.sModeName),modenames[st.nMode]);

		snprintf(filespec,FILESPECLEN
				,"data/%s%d.%s"
				,cf.telemfilebase
				,st.nId
				,cf.telemfileext);		
	}

	if (g->status > 0){
		if ((g->fix.mode > 1)){
			if(isnan(g->fix.time)){
				printf("time is NAN\n");
			}else{

				printf("here4\n");
		
				st.nTime = (long int)(g->fix.time);
				tmp = localtime(&(st.nTime));		
				status_SetTimeString(st.sTime, sizeof(st.sTime),tmp);

				printf("here5\n");

				st.dLatDeg = g->fix.latitude;
				st.dLonDeg = g->fix.longitude;

				st.dAltMeters = g->fix.altitude;       
				st.dSpeed_mps = g->fix.speed;


//			snprintf(cf.telemfilebase,CFGMAXFILENAMELENGTH,"telemfile");
//			snprintf(cf.telemfileext,CFGMAXFILENAMELENGTH,"txt");

				printf("here6\n");
				telemfile_WriteFile(cf,st);
			
				printf("here7\n");
				sendretr_PutFile(cf, filespec);
			}
		}
	}

	printf("here8\n");
	
	return;
}

/*************************
*
*
*************************/
static void printGPSdata(
		GPSDATA gpsdata)
{

	#ifdef DEBUG
	logfile_LogPrint("--------------");
	switch(gpsdata->status){
		case STATUS_NO_FIX:
			logfile_LogPrint("  status = No fix");
			break;
		case STATUS_FIX:
			logfile_LogPrint("  status = GPS fix");
			break;		
		case STATUS_DGPS_FIX:
			logfile_LogPrint("  status = DGPS fix");
			break;
	}

	if (gpsdata->status > 0){

		switch(gpsdata->fix.mode){
			case MODE_NOT_SEEN:
				logfile_LogPrint("  mode   = MODE_NOT_SEEN");
				break;
			case MODE_NO_FIX:
				logfile_LogPrint("  mode   = No fix");
				break;
			case MODE_2D:
				logfile_LogPrint("  mode   = 2D fix");
				break;		
			case MODE_3D:
				logfile_LogPrint("  mode   = 3D fix");
				break;
		}

		if ((gpsdata->fix.mode > 1)){
			char str[LOGSTRLEN];
			
			/* Latitude in degrees (valid if mode >= 2) */
			snprintf(str,LOGSTRLEN,"  lat    = %.7f deg", gpsdata->fix.latitude );
			logfile_LogPrint(str);

			/* Longitude in degrees (valid if mode >= 2) */
			snprintf(str,LOGSTRLEN,"  lon    = %.7f deg", gpsdata->fix.longitude );
			logfile_LogPrint(str);

			/* Altitude in meters (valid if mode == 3) */
			snprintf(str,LOGSTRLEN,"  alt    = %.3f m", gpsdata->fix.altitude );
			logfile_LogPrint(str);

			/* Speed over ground, meters/sec */
			snprintf(str,LOGSTRLEN,"  speed  = %.3f m/s", gpsdata->fix.speed );
			logfile_LogPrint(str);

			/* Vertical speed, meters/sec */
			snprintf(str,LOGSTRLEN,"  climb  = %.3f m/s", gpsdata->fix.climb );
			logfile_LogPrint(str);

			/* Time of update, seconds since Unix epoch */
			snprintf(str,LOGSTRLEN,"  time   = %.0f s", gpsdata->fix.time );
			logfile_LogPrint(str);

			/* Horizontal position uncertainty, meters */
			snprintf(str,LOGSTRLEN,"  eph   = %.7f m", gpsdata->fix.eph );
			logfile_LogPrint(str);

			/* Vertical position uncertainty, meters */
			snprintf(str,LOGSTRLEN,"  epv   = %.7f m", gpsdata->fix.epv );
			logfile_LogPrint(str);

			/* Speed uncertainty, meters/sec */
			snprintf(str,LOGSTRLEN,"  eps   = %.7f m/s", gpsdata->fix.eps );
			logfile_LogPrint(str);

			/* Vertical speed uncertainty */
			snprintf(str,LOGSTRLEN,"  epc   = %.7f m/s", gpsdata->fix.epc );
			logfile_LogPrint(str);


//    double ept;		/* Expected time uncertainty */
//    double track;	/* Course made good (relative to true north) */
//    double epd;		/* Track uncertainty, degrees */

			/* Number of satellites used in solution */
			snprintf(str,LOGSTRLEN,"  # sat        = %d", (unsigned int)(gpsdata->satellites) );
			logfile_LogPrint(str);

			/* Number of satellites used in solution */
			snprintf(str,LOGSTRLEN,"  # sat used   = %d", gpsdata->satellites_used );
			logfile_LogPrint(str);
			
/*
			if(gpsdata->satellites_used >0){
				register int i;
				
				printf("    sat list = (" );
				for(i=0;i<MAXCHANNELS;i++){
					/* PRNs of satellites used in solution */
/*					printf("%c, ", gpsdata->used[i]?'Y':'N' );
				}
				printf(")");			
			}
			printf("");			
*/			
			/* Dilution of precision */
			snprintf(str,LOGSTRLEN,"  pdop   = %.3f ", gpsdata->pdop );
			logfile_LogPrint(str);
			snprintf(str,LOGSTRLEN,"  hdop   = %.3f ", gpsdata->hdop );
			logfile_LogPrint(str);
			snprintf(str,LOGSTRLEN,"  vdop   = %.3f ", gpsdata->vdop );
			logfile_LogPrint(str);
			snprintf(str,LOGSTRLEN,"  tdop   = %.3f ", gpsdata->tdop );
			logfile_LogPrint(str);
			snprintf(str,LOGSTRLEN,"  gdop   = %.3f ", gpsdata->gdop );
			logfile_LogPrint(str);

		}
	}
	#endif
	
	return;
}


/*************************
*
*
*************************/
void gpsCallback(
		GPSDATA gpsdata,
		char *buf, 
		size_t len, 
		int level)
{
	GPSDATA g;

	g = gpsdata;
	
//	printf("GPSD: %s\n", buf);
//	fflush(stdout);

	printGPSdata(g);
	writeTelemFile(cf0,g);
	return;
}


/*************************
*
*
*************************/
static void gpsInit(char *host,char *port)
{
	#ifdef DEBUG
	logfile_LogPrint("Initializing gps connection");
	#endif
	printf("Initializing gps connection\n");


	gpsdata0 = gps_open(host, port);

	if (!gpsdata0) {
		char *err_str;
		switch (errno) {
		case NL_NOSERVICE:
			err_str = "can't get service entry";
			break;
		case NL_NOHOST:
			err_str = "can't get host entry";
			break;
		case NL_NOPROTO:
			err_str = "can't get protocol entry";
			break;
		case NL_NOSOCK:
			err_str = "can't create socket";
			break;
		case NL_NOSOCKOPT:
			err_str = "error SETSOCKOPT SO_REUSEADDR";
			break;
		case NL_NOCONNECT:
			err_str = "can't connect to host";
			break;
		default:
			err_str = "Unknown";
			break;
		}
		fprintf(stderr,
		    "cgpxlogger: no gpsd running or network error: %d, %s",
		    errno, err_str);
		exit(1);
	}

	printf("   Success!\n");

	#ifdef DEBUG
	logfile_LogPrint("   Success!");
	#endif

	gps_set_raw_hook(gpsdata0, gpsCallback);

	return;
}


/*************************
*
*
*************************/
int mainloop()
{	
	TIMESPEC delay;
	fd_set fds;
	struct timeval tv;
	int data;
	char *pollstr = "SPAMDQTV";

	delay = delay_setdelay(2.0);

	
	while(1){
		FD_ZERO(&fds);
		FD_SET(gpsdata0->gps_fd, &fds);

		printf("here\n");

		if(gps_query(gpsdata0, pollstr) != 0){
				printf("ack2!\n");
				exit(-2);
		}

		printf("here3\n");

		tv.tv_usec = 250000;
		tv.tv_sec = 0;
		data = select(gpsdata0->gps_fd + 1, &fds, NULL, NULL, &tv);

		printf("here1\n");
		if (data < 0) {
			printf("ack!\n");
			fprintf(stderr,"%s", strerror(errno));
			exit(2);
		}else if (data){
			if(gps_poll(gpsdata0) != 0){
				printf("ack1!\n");
				exit(-2);
			}
		}
		
		printf("here2\n");
		delay_sleep(&delay);
	}	
}


/*************************
*
*
*************************/
void gpsSetIntCallback()
{
	signal(SIGINT, gpsInterruptCallback);
	signal(SIGTERM, gpsInterruptCallback);
	signal(SIGQUIT, gpsInterruptCallback);
	signal(SIGHUP, gpsInterruptCallback);
}


/*************************
*
*
*************************/
void gpsOpenLogfile(char *logfilename)
{
	#ifdef DEBUG
	logfile_OpenFile(logfilename);
	logfile_LogPrint(" ");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint("GPS STARTUP");
	logfile_LogPrint("*******************************************");		
	logfile_LogPrint(" ");
	#endif

	return;
}

/*************************
*
*
*************************/
int main(int argc, char **argv)
{
	char *host = "127.0.0.1";
	char *port = "2947";
	
	cf0 = config_readconfig(argc,argv,CFGFILENAME);		

	gpsOpenLogfile(cf0.logfilename);
		
	gpsSetIntCallback();

	gpsInit(host,port);
	
	mainloop();

	return(0);
}

